/*
File: get_rgb_depth.cpp 
Usage: Capture rgb image and depth image from online data stream
Author: Qiao Zhao
Date: May 30th, 2011
*/
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/JointState.h"
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include "pcl/io/io.h"
#include "pcl/io/pcd_io.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <boost/thread/thread.hpp>
#include <trajectory_msgs/JointTrajectory.h>
#include <iostream>
#include <stdio.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#define METER_PER_UNIT 0.04
using namespace std;
using namespace cv;
namespace enc = sensor_msgs::image_encodings;

//create a thread to publish marked image.

//rosbag record /camera/depth/camera_info /camera/depth/image /camera/rgb/points /camera/depth/points /camera/rgb/image_color /tf
//rosbag play --loop --clock --start=4 qz2.bag

sensor_msgs::Image depth_msg;
sensor_msgs::Image rgb_msg;

CascadeClassifier cascade;
bool depth_ready=false;
bool rgb_ready=false;
double scale = 1;
int imgcnt=0;
char dir[]="dual_image";



void getDepth(const sensor_msgs::Image& msg) {
    depth_msg=msg;
    depth_ready=true;
}

void getRGB(const sensor_msgs::Image& msg) {
    rgb_msg=msg;
    rgb_ready=true;
}



void detectAndMark() {
    ros::Rate rt(1);
    if (!depth_ready||!rgb_ready)
        rt.sleep();
    char buffer[50];
    cv_bridge::CvImagePtr depth_ptr;
    cv_bridge::CvImagePtr rgb_ptr;
    int height=depth_msg.height;
    int width=depth_msg.width;
    unsigned char* data=new unsigned char[height*width*4];
    for (;;) {
        for (unsigned int j=0;j<depth_msg.data.size();j++)
            data[j]=depth_msg.data[j];
        float* ptr=(float*)data;
	for (int i=0;i<height*width;i++){
	    //float t=*(ptr+i);
	    if(isnan(ptr[i]))
		//*(ptr+i)=0;
		ptr[i]=0;
	    else
		//*(ptr+i)=*(ptr+i)/METER_PER_UNIT;
		ptr[i]=ptr[i]/METER_PER_UNIT;
	}
	cv::Mat Ma=cv::Mat(height, width, CV_32FC1, ptr);
	imgcnt++;
	sprintf(buffer,"%s/%ddepth.jpg",dir,imgcnt);
	cv::imwrite(buffer,Ma);
        try
        {
            rgb_ptr = cv_bridge::toCvCopy(rgb_msg, enc::BGR8);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }
        sprintf(buffer,"%s/%drgb.jpg",dir,imgcnt);
	cv::imwrite(buffer,rgb_ptr->image);
        Mat& img=rgb_ptr->image;
        int i = 0;
        double t = 0;
        vector<Rect> faces;
        const static Scalar colors[] =  { CV_RGB(0,0,255),
                                          CV_RGB(0,128,255),
                                          CV_RGB(0,255,255),
                                          CV_RGB(0,255,0),
                                          CV_RGB(255,128,0),
                                          CV_RGB(255,255,0),
                                          CV_RGB(255,0,0),
                                          CV_RGB(255,0,255)
                                        } ;
        Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );

        cvtColor( img, gray, CV_BGR2GRAY );
        resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
        equalizeHist( smallImg, smallImg );

        t = (double)cvGetTickCount();
        cascade.detectMultiScale( smallImg, faces,
                                  1.1, 2, 0
                                  //|CV_HAAR_FIND_BIGGEST_OBJECT
                                  //|CV_HAAR_DO_ROUGH_SEARCH
                                  |CV_HAAR_SCALE_IMAGE
                                  ,
                                  Size(30, 30) );
        t = (double)cvGetTickCount() - t;
        printf( "detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );
        for ( vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++ )
        {
            Point center;
            Scalar color = colors[i%8];
            int radius;
            center.x = cvRound((r->x + r->width*0.5)*scale);
            center.y = cvRound((r->y + r->height*0.5)*scale);
            radius = cvRound((r->width + r->height)*0.25*scale);
            circle( img, center, radius, color, 3, 8, 0 );
           

        }
        cv::imshow( "result", img );
        cv::waitKey(0);
        rt.sleep();
    }
    delete data;
}



int main( int argc, char** argv )
{

    ros::init(argc, argv, "tracker");
    ros::NodeHandle n;
    cascade.load( "haarcascade_frontalface_alt.xml" );
    cvNamedWindow( "result", 1 );
    
    ros::Subscriber sub_depth = n.subscribe("/camera/depth/image", 10, getDepth);
    ros::Subscriber sub_rgb = n.subscribe("/camera/rgb/image_color", 10, getRGB);
    
    ros::Rate rt(1);
    rt.sleep();
    boost::thread detect_thread = boost::thread::thread(boost::bind(&detectAndMark));
   
    ros::spin();
    cvDestroyWindow("result");
    return 0;
}

//     for (int r = (int)faces.size() - 1; r >= 0; r--)
//     {
//         double depth;
//         depth = depthimg.at<float>(faces[r].y + faces[r].height/2, faces[r].x + faces[r].width/2);
//         bool flag = CheckFaceDepth(depth, faces[r].width);
//         if (!flag)
//             faces.erase(faces.begin() + r);
//     }
// 
//     for (int r = (int)bodies.size() - 1; r >= 0; r--)
//     {
//         double depth;
//         depth = depthimg.at<float>(bodies[r].y + bodies[r].height/2, bodies[r].x + bodies[r].width/2);
//         bool flag = CheckFaceDepth(depth, bodies[r].width);
//         if (!flag)
//             bodies.erase(bodies.begin() + r);
//     }